/**
 * @file /src/qnode.cpp
 *
 * @brief Ros communication central!
 *
 * @date February 2011
 **/

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ros/ros.h>
#include <ros/network.h>
#include <string>
#include <std_msgs/String.h>
#include <sstream>
#include "../include/gui/qnode.hpp"
#include "include/gui/ui_data.h"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace gui {

/*****************************************************************************
** Implementation
*****************************************************************************/

QNode::QNode(int argc, char** argv ) :
	init_argc(argc),
	init_argv(argv)
  {
    bool res = init();
    if(res)
    {
      log(Info,"qnode init success!");
    }
  }

QNode::~QNode() {
    if(ros::isStarted()) {
      ros::shutdown(); // explicitly needed since we use ros::start();
      ros::waitForShutdown();
    }
	wait();
}

bool QNode::init() {
	ros::init(init_argc,init_argv,"gui");
	if ( ! ros::master::check() ) {
		return false;
	}
	ros::start(); // explicitly needed since our nodehandle is going out of scope.
	ros::NodeHandle n;
	// Add your ros communications here.
//  chatter_publisher = n.advertise<std_msgs::String>("chatter", 10);

  chatter_publisher = n.advertise<std_msgs::String>("chatter", 1000);
//  pc_robot_status_run_re_sub_ = n.subscribe<agv_msgs::pc_robot_status_run>("agv_run_status",1000,&QNode::pcRobotStatusRunCallback,this);
//  agv_loc_info_sub_ = n.subscribe<agv_msgs::agv_loc_cmd>("agv_loc_info",1,&QNode::agvLocInfoCallback,this);

  chatter_publisher = n.advertise<std_msgs::String>("chatter", 10);
  pc_robot_status_run_re_sub_ = n.subscribe<agv_msgs::pc_robot_status_run>("agv_error",1000,&QNode::pcRobotStatusRunCallback,this);
  agv_loc_info_sub_ = n.subscribe<agv_msgs::agv_loc_cmd>("agv_loc_info",10,&QNode::agvLocInfoCallback,this);
  screen_para_sub_ = n.subscribe<agv_msgs::screen_param>("screen_para", 10,&QNode::screenParaCallback,this);
  arm_run_info_sub_ = n.subscribe<agv_msgs::arm_run_info>("arm_run_info",10,&QNode::armRunInfoCallback,this);

  debug_sub_ = n.subscribe<std_msgs::Int32>("debug",10,&QNode::debugCallback,this);
  agv_loc_status_sub_ = n.subscribe<std_msgs::Int32>("agv_loc_status",10,&QNode::agvLocStatusCallback,this);
  network_info_sub_ = n.subscribe<std_msgs::Int32>("net_status",10,&QNode::networkInfoStatusCallback,this);
  //抱手车打开注释
  communication_check_sub_ = n.subscribe<agv_msgs::communication_check>("communication_check",10,&QNode::CommunicationcCheckCallback,this);
 //叉车打开注释
//  communication_check_mianbao_sub_ = n.subscribe<std_msgs::Int32>("communication_check",10,&QNode::CommunicationcCheckMianBaoCallback,this);
  version_sub_ = n.subscribe<std_msgs::String>("embedded_version",10,&QNode::versionCallback,this);

  mechanical_param_sub_ = n.subscribe<agv_msgs::mech_param>("mech_param",10,&QNode::mechParamCallback,this);
  arm_operation_info_sub_ = n.subscribe<agv_msgs::arm_operation_info>("arm_operation_info",10,&QNode::armOperationInfoCallback,this);
  mission_info_sub_ = n.subscribe<agv_msgs::mission_info>("mission_info",10,&QNode::missionInfoCallback,this);
  device_status_sub_ = n.subscribe<agv_msgs::device_status>("device_status",10,&QNode::deviceStatusCallback,this);

  //add new
  agv_running_info_sub_ = n.subscribe<agv_msgs::running_info>("running_info",10,&QNode::agvRunningInfoCallback,this);
  io_send_sub_ = n.subscribe<agv_msgs::io_send>("IO_status",10,&QNode::ioSendCallback,this);
  motor_info_re_sub_ = n.subscribe<agv_msgs::motor_info_re>("motor_info_re",1000,&QNode::motorInforeCallback,this);
  battery_info_re_sub_ = n.subscribe<agv_msgs::battery_info>("battery_info_re",10,&QNode::batteryInforeCallback,this);
  agvsw_status_sub_ = n.subscribe<agv_msgs::agvswstate>("agvsw_state",10,&QNode::agvSwStatusCallback,this);
  log_info_sub_ = n.subscribe<agv_msgs::log_info>("agv_log_info",10,&QNode::logInfoCallback,this);
  radar_bucket_info_sub_ = n.subscribe<agv_msgs::radar_bucket_info>("radar_bucket", 10, &QNode::radarBucketInfoCallback, this);
  gyro_info_sub_ = n.subscribe<agv_msgs::gyro_s>("gyro_s", 10, &QNode::gyroInfoCallback, this);
  selftask_info_sub_ = n.subscribe<agv_msgs::selftask_info>("selftask_info", 10, &QNode::selftaskInfocallback, this);
  remote_control_re_sub_ = n.subscribe<std_msgs::Int32>("remote_switch",10,&QNode::remoteControlCallback, this);
  pc_info_re_sub_ = n.subscribe<agv_msgs::pc_info>("pc_info", 10, &QNode::pcInfoCallback, this);
  sys_config_re_sub_ = n.subscribe<agv_msgs::sys_config>("sys_config_re", 100, &QNode::sysConfigCallback, this);
  ecoline_info_sub_ = n.subscribe<agv_msgs::ecoline_info>("ecoline_info", 100, &QNode::ecolineInfoCallback, this);

  mech_param_pub_ = n.advertise<agv_msgs::send_mech_para>("sendmechpara",10);
  control_send_pub_ = n.advertise<agv_msgs::control_info_send>("screen_cmd",10);
  errmask_send_pub_ = n.advertise<agv_msgs::mask>("mask_error",10);
  arm_open_close_pub_ = n.advertise<agv_msgs::arm_cmd>("arm_open_close",10);
  quick_fixed_position_send_pub_ = n.advertise<agv_msgs::quick_fixed_position_send>("quick_fixed_position", 10);
  cancel_task_send_pub_ = n.advertise<agv_msgs::mission_cancel>("self_missioin_cancel",10);
  robot_pause_send_pub_ = n.advertise<agv_msgs::robot_pause>("robot_pause", 10);
  load_upload_cargo_pub_ = n.advertise<agv_msgs::load_upload_cargo>("agv_load_upload_cargo",10);
  led_show_send_pub_ = n.advertise<agv_msgs::led_show>("led_show", 100);
  sys_config_send_pub_ = n.advertise<agv_msgs::sys_config>("sys_config_send", 100);

//  cancel_task_send_pub_ = n.advertise<std_msgs::Int32>("cancel_task", 100);
  reboot_agv_send_pub_ = n.advertise<std_msgs::Int32>("reboot_agv", 100);
  qt_jackup_operate_pub_ = n.advertise<std_msgs::Int32>("qt_jackup_operate",100);
  qt_up_down_pub_ = n.advertise<agv_msgs::sensor_control>("jack_up",100);
  clean_mode_send_pub_ = n.advertise<std_msgs::Int32MultiArray>("clean_mode",100);
  self_task_cmd_pub_ = n.advertise<agv_msgs::self_task_cmd>("self_task_cmd", 100);

	start();
	return true;
}

bool QNode::init(const std::string &master_url, const std::string &host_url) {
  std::map<std::string,std::string> remappings;
  remappings["__master"] = master_url;
  remappings["__hostname"] = host_url;

  ros::init(remappings,"gui");
  if ( ! ros::master::check() ) {
    return false;
  }
  ros::start(); // explicitly needed since our nodehandle is going out of scope.
  ros::NodeHandle n;
  // Add your ros communications here.

  start();
  return true;
}

//接收回调函数
void QNode::pcRobotStatusRunCallback(agv_msgs::pc_robot_status_run msg)
{
//  log(Info,"pcRobotStatusRunCallback");
  usb_robot_status_run_re_ = msg;
}
void QNode::agvLocInfoCallback(agv_msgs::agv_loc_cmd msg)
{
//  log(Info,"agvLocInfoCallback");
  agv_loc_info_re_ = msg;
}
void QNode::screenParaCallback(agv_msgs::screen_param msg)
{
//  log(Info,"screenParaCallback");
  screen_para_re_ = msg;
}
void QNode::armRunInfoCallback(const agv_msgs::arm_run_info::ConstPtr& msg)
{
//  log(Info,"armRunInfoCallback");
  arm_run_info_re_ = *msg;
}

void QNode::debugCallback(const std_msgs::Int32::ConstPtr& msg)
{
  log(Info,"debugcallback");
}

void QNode::mechParamCallback(agv_msgs::mech_param msg)
{
//  log(Info,"mechParamCallback");
  mech_param_ = msg;
}

void QNode::armOperationInfoCallback(agv_msgs::arm_operation_info msg)
{
//  log(Info,"armOperationInfoCallback");
}

void QNode::missionInfoCallback(agv_msgs::mission_info msg)
{
//  log(Info,"missionInfoCallback");
  mission_info_ = msg;
}

void QNode::deviceStatusCallback(agv_msgs::device_status msg)
{
  log(Info,"deviceStatusCallback");
}

void QNode::agvRunningInfoCallback(agv_msgs::running_info msg)
{
//  log(Info,"reciveagvRunningInfoCallback");
  running_info_ = msg;
}

void QNode::ioSendCallback(agv_msgs::io_send msg)
{
//  log(Info,"ioSendCallback");
  io_send_ = msg;
  if(io_send_.Num == 100)
  {
    io_send_ultr = msg;
  }
  else
  {
    io_send_IO = msg;
  }
}

void QNode::motorInforeCallback(agv_msgs::motor_info_re msg)
{
//  log(Info,"motorInforeCallback");
  motor_info_re_ = msg;
  if(motor_info_re_.serialNum == 1)
  {
    motor_info_re_Run[0] = msg;
  }
  else if(motor_info_re_.serialNum == 2)
  {
    motor_info_re_Run[1] = msg;
  }
  else if(motor_info_re_.serialNum == 3)
  {
    motor_info_re_Run[2] = msg;
  }
  else if(motor_info_re_.serialNum == 4)
  {
    motor_info_re_Run[3] = msg;
  }
  else if(motor_info_re_.serialNum == 5)
  {
    motor_info_re_Run[4] = msg;
  }
  else if(motor_info_re_.serialNum == 6)
  {
    motorNumMode = 1;
    motor_info_re_Run[5] = msg;
  }

}

void QNode::batteryInforeCallback(agv_msgs::battery_info msg)
{
//    log(Info,"batteryInforeCallback");
  battery_info_re_ = msg;
}

//抱手车打开
void QNode::CommunicationcCheckCallback(agv_msgs::communication_check msg)
{
  communication_check_re_ = msg;
}

void QNode::CommunicationcCheckMianBaoCallback(const std_msgs::Int32 msg)
{
  communication_check_mianbao_re_ = msg;
}

void QNode::networkInfoStatusCallback(const std_msgs::Int32 msg)
{
  network_info_re_ = msg;
}

void QNode::agvLocStatusCallback(const std_msgs::Int32 msg)
{
  agv_loc_info = msg;
}

void QNode::versionCallback(const std_msgs::String msg)
{
//  log(Info,"versionInforeCallback");
//  from_arm_check_connection_++;
//  if(from_arm_check_connection_ % 3 == 0)
//  {
//    from_arm_check_connection++;
//  }
  version = msg;
}

void QNode::agvSwStatusCallback(agv_msgs::agvswstate msg)
{
//  log(Info,"agvsw_stateCallback");
  agvsw_status_re_ = msg;
}

void QNode::logInfoCallback(agv_msgs::log_info msg)
{
////  log(Info,"logInfoCallback");
//  from_pc_log_info_status = 1;
//  QMutexLocker mutexLocker(&mutex);
//  log_info_re_ = msg;

//  Q_EMIT logShow();

}

void QNode::radarBucketInfoCallback(agv_msgs::radar_bucket_info msg)
{
//  log(Info,"radarBucketInfoCallback");
  radar_bucket_info_re_ = msg;
}

void QNode::gyroInfoCallback(agv_msgs::gyro_s msg)
{
//  log(Info,"gyroInfoCallback");
  gyro_info_re_ = msg;
}

void QNode::selftaskInfocallback(agv_msgs::selftask_info msg)
{
//  selftask_show_status = 1;
//  log(Info,"selftaskInfocallback");
//  selftask_info_re = msg;

//  Q_EMIT logShow();
}

void QNode::remoteControlCallback(const std_msgs::Int32 msg)
{
//  log(Info,"selftaskInfocallback");
  remote_control_re_ = msg;
}

void QNode::pcInfoCallback(agv_msgs::pc_info msg)
{
  //  log(Info,"pc_info_re_");
  pc_info_re_ = msg;
}

void QNode::sysConfigCallback(agv_msgs::sys_config msg)
{
//  log(Info,"sysConfigCallback");
  sys_config_re_ = msg;
}

void QNode::ecolineInfoCallback(agv_msgs::ecoline_info msg)
{
//  log(Info,"ecolineInfoCallback");
  ecoline_info_re_ = msg;
  if(ecoline_info_re_.serialNum == 1)
  {
    ecoline_info_re_num_[0] = msg;
  }
  else if(ecoline_info_re_.serialNum == 2)
  {
    ecoline_info_re_num_[1] = msg;
  }
  else if(ecoline_info_re_.serialNum == 3)
  {
    ecoline_info_re_num_[2] = msg;
  }
}

//send回调函数
void QNode::MechParamPublish(agv_msgs::send_mech_para msg)
{
//  log(Info,"MechParamPublish");
  mech_param_pub_.publish(msg);
}

void QNode::controlSendPublish(agv_msgs::control_info_send msg)
{
  log(Info,"controlSendPublish");
  control_send_pub_.publish(msg);
}

void QNode::errmaskSendPublish(agv_msgs::mask msg)
{
  log(Info,"errmaskSendPublish");
  errmask_send_pub_.publish(msg);
}

void QNode::armcmdSendPublish(agv_msgs::arm_cmd msg)
{
  log(Info, "armcmdsend");
  arm_open_close_pub_.publish(msg);
}

void QNode::quickFixedPositionSendPublish(agv_msgs::quick_fixed_position_send msg)
{
  log(Info, "quickFixedPositionSendPublish");
  quick_fixed_position_send_pub_.publish(msg);
}

void QNode::cancelTaskSendPublish(const std_msgs::Int32 msg)
{
//  log(Info, "cancelTaskSendPublish");
//  cancel_task_send_pub_.publish(msg);
}

void QNode::rebootAgvSendPublish(const std_msgs::Int32 msg)
{
  log(Info, "rebootAgvSendPublish");
  reboot_agv_send_pub_.publish(msg);
}

void QNode::missionCancelSendCallback(agv_msgs::mission_cancel msg)
{
  log(Info, "missionCancelSendCallback");
  cancel_task_send_pub_.publish(msg);
}

void QNode::robotPauseSendCallback(agv_msgs::robot_pause msg)
{
  log(Info, "robotPauseSendCallback");
  robot_pause_send_pub_.publish(msg);
}

//装货、卸货任务完成话题
void QNode::LoadUploadCargoPulish(agv_msgs::load_upload_cargo msg)
{
  log(Info, "LoadUploadCargoPulish");
  load_upload_cargo_pub_.publish(msg);
}

void QNode::QtJackupOperatePublish(const std_msgs::Int32 msg)
{
  log(Info,"QtJackupOperatePublish");
  qt_jackup_operate_pub_.publish(msg);
}

void QNode::QtUpDownPublish(agv_msgs::sensor_control msg)
{
  log(Info,"QtUpDownPublish");
  qt_up_down_pub_.publish(msg);
}

void QNode::ledShowSendPublish(agv_msgs::led_show msg)
{
//  log(Info, "ledShowSendPublish");
  led_show_send_pub_.publish(msg);
}

void QNode::sysConfigSendPublish(agv_msgs::sys_config msg)
{
//  log(Info, "sysConfigSendCallback");
  sys_config_send_pub_.publish(msg);
}

void QNode::cleanModeSendPublish(std_msgs::Int32MultiArray msg)
{
  log(Info, "cleanModeSendPublish");
  clean_mode_send_pub_.publish(msg);
}

void QNode::selfTaskSendPublish(agv_msgs::self_task_cmd msg)
{
  log(Info, "selfTaskSendPublish");
  self_task_cmd_pub_.publish(msg);
}

void QNode::run() {
	ros::Rate loop_rate(1);
	int count = 0;
	while ( ros::ok() ) {

		std_msgs::String msg;
		std::stringstream ss;
		ss << "hello world " << count;
		msg.data = ss.str();
		chatter_publisher.publish(msg);
//		log(Info,std::string("I sent: ")+msg.data);
		ros::spinOnce();
		loop_rate.sleep();
		++count;
	}
	std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
	Q_EMIT rosShutdown(); // used to signal the gui for a shutdown (useful to roslaunch)
    Q_EMIT guiShutdown();
}


void QNode::log( const LogLevel &level, const std::string &msg) {
	logging_model.insertRows(logging_model.rowCount(),1);
	std::stringstream logging_model_msg;
	switch ( level ) {
		case(Debug) : {
				ROS_DEBUG_STREAM(msg);
				logging_model_msg << "[DEBUG] [" << ros::Time::now() << "]: " << msg;
				break;
		}
		case(Info) : {
				ROS_INFO_STREAM(msg);
				logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
				break;
		}
		case(Warn) : {
				ROS_WARN_STREAM(msg);
				logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
				break;
		}
		case(Error) : {
				ROS_ERROR_STREAM(msg);
				logging_model_msg << "[ERROR] [" << ros::Time::now() << "]: " << msg;
				break;
		}
		case(Fatal) : {
				ROS_FATAL_STREAM(msg);
				logging_model_msg << "[FATAL] [" << ros::Time::now() << "]: " << msg;
				break;
		}
	}
	QVariant new_row(QString(logging_model_msg.str().c_str()));
	logging_model.setData(logging_model.index(logging_model.rowCount()-1),new_row);
	Q_EMIT loggingUpdated(); // used to readjust the scrollbar
}

}  // namespace gui
